import rclpy
from geometry_msgs.msg import PoseStamped, Pose#位姿和时间戳
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult#BasicNavigator是基本导航模块,TaskResult是枚举类型,用于判断任务的成功与否
from tf2_ros import TransformListener, Buffer#tf监听者和缓冲区存储接收的tf数据
from tf_transformations import euler_from_quaternion, quaternion_from_euler#欧拉四元数互相转换
from rclpy.duration import Duration
from autopatrol_interfaces.srv import SpeachText#语音模块的消息接口
from sensor_msgs.msg import Image#图片消息接口,用于相机模块
from cv_bridge import CvBridge#允许你在 ROS 图像消息（sensor_msgs/Image 类型）和 OpenCV 图像（numpy 数组格式）之间进行转换
import cv2

class PatrolNode(BasicNavigator):
    def __init__(self, node_name='patrol_node'):
        super().__init__(node_name)
        # 导航相关定义
        self.declare_parameter('initial_point', [0.0, 0.0, 0.0])
        self.declare_parameter('target_points', [0.0, 0.0, 0.0, 1.0, 1.0, 1.57])#声明参数,初始位置和结束位置,目标位置有俩点
        self.initial_point_ = self.get_parameter('initial_point').value
        self.target_points_ = self.get_parameter('target_points').value
        # 实时位置获取 TF 相关定义
        self.buffer_ = Buffer()
        self.listener_ = TransformListener(self.buffer_, self)#创建监听者

        #创建语音模块客户端
        self.speach_client_ = self.create_client(SpeachText, 'speech_text')#创建语音模块的客户端,接口为SpeachText

        # 订阅与保存图像相关定义
        self.declare_parameter('image_save_path', '')#声明参数,保存图像路径,一开始为空
        self.image_save_path = self.get_parameter('image_save_path').value
        self.bridge = CvBridge()#初始化转换中介
        self.latest_image = None#上一张图片,初始化为空
        self.subscription_image = self.create_subscription(
            Image, '/camera_sensor/image_raw', self.image_callback, 10)#创建相机模块的订阅者,这个话题是仿真部分的camera_sensor发布的

    def image_callback(self, msg):
        """
        将最新的消息放到 latest_image 中
        """
        self.latest_image = msg#更新照片信息的函数

    def record_image(self):#记录图像的函数
        """
        记录图像
        """
        if self.latest_image is not None:#要是上一张照片不为空
          pose = self.get_current_pose()#先获取当前位姿
          cv_image = self.bridge.imgmsg_to_cv2(self.latest_image)#通过桥梁转换图片格式
          cv2.imwrite(f'{self.image_save_path}image_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png', cv_image)#将图片写入指定路径

    def speach_text(self, text):
        """
        调用服务播放语音
        """
        while not self.speach_client_.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('语合成服务未上线，等待中。。。')

        """
        request:
        string text    # 合成文字
        ---
        result:
        bool result    # 合成结果
        """
        request = SpeachText.Request()#初始化请求部分
        request.text = text#这个就是传进来的说话参数
        future = self.speach_client_.call_async(request)#异步调用,未来结果
        rclpy.spin_until_future_complete(self, future)

        if future.result() is not None:#结果不为空
            result = future.result().result#赋值结果部分
            if result:
                self.get_logger().info(f'语音合成成功：{text}')
            else:
                self.get_logger().warn(f'语音合成失败：{text}')
        else:
            self.get_logger().warn('语音合成服务请求失败')

    def get_pose_by_xyyaw(self, x, y, yaw):#合成位姿函数
        """
        通过 x,y,yaw 合成 PoseStamped
        #goal definition
        geometry_msgs/PoseStamped pose
            std_msgs/Header header
                builtin_interfaces/Time stamp
                    int32 sec
                    uint32 nanosec
                string frame_id
            Pose pose
                Point position
                    float64 x
                    float64 y
                    float64 z
                Quaternion orientation
                    float64 x 0
                    float64 y 0
                    float64 z 0
                    float64 w 1
        string behavior_tree
        """
        pose = PoseStamped()#创建类
        pose.header.frame_id = 'map'#指定坐标系
        pose.pose.position.x = x
        pose.pose.position.y = y
        rotation_quat = quaternion_from_euler(0, 0, yaw)
        pose.pose.orientation.x = rotation_quat[0]
        pose.pose.orientation.y = rotation_quat[1]
        pose.pose.orientation.z = rotation_quat[2]
        pose.pose.orientation.w = rotation_quat[3]
        return pose#位姿

    def init_robot_pose(self):
        """
        初始化机器人位姿
        """
        # 从参数获取初始化点
        self.initial_point_ = self.get_parameter('initial_point').value
        # 合成位姿并进行初始化
        self.setInitialPose(self.get_pose_by_xyyaw(
            self.initial_point_[0], self.initial_point_[1], self.initial_point_[2]))
        # 等待直到导航激活
        self.waitUntilNav2Active()

    def get_target_points(self):
        """
        通过参数值获取目标点集合        
        """
        points = []#存点
        self.target_points_ = self.get_parameter('target_points').value
        for index in range(int(len(self.target_points_)/3)):
            x = self.target_points_[index*3]
            y = self.target_points_[index*3+1]
            yaw = self.target_points_[index*3+2]
            points.append([x, y, yaw])
            self.get_logger().info(f'获取到目标点: {index}->({x},{y},{yaw})')
        return points

    def nav_to_pose(self, target_pose):
        """
        导航到指定位姿
        """
        self.waitUntilNav2Active()#等待导航服务启动
        result = self.goToPose(target_pose)#调用位姿并且获取结果
        while not self.isTaskComplete():#还没有结束的时候
            feedback = self.getFeedback()#获取反馈
            if feedback:#反馈不为空,播报
                self.get_logger().info(f'预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达')
        # 最终结果判断
        result = self.getResult()
        if result == TaskResult.SUCCEEDED:
            self.get_logger().info('导航结果：成功')
        elif result == TaskResult.CANCELED:
            self.get_logger().warn('导航结果：被取消')
        elif result == TaskResult.FAILED:
            self.get_logger().error('导航结果：失败')
        else:
            self.get_logger().error('导航结果：返回状态无效')

    def get_current_pose(self):#获取当前位姿
        """
        通过TF获取当前位姿
        """
        while rclpy.ok():
            try:
                tf = self.buffer_.lookup_transform(
                    'map', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1))#读取
                transform = tf.transform#读取数据并存储
                rotation_euler = euler_from_quaternion([
                    transform.rotation.x,
                    transform.rotation.y,
                    transform.rotation.z,
                    transform.rotation.w
                ])#进行转换
                self.get_logger().info(
                    f'平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}')
                return transform
            except Exception as e:
                self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}')
    
def main():
    rclpy.init()
    patrol = PatrolNode()#初始化
    patrol.speach_text(text='正在初始化位置')#调用方法
    patrol.init_robot_pose()
    patrol.speach_text(text='位置初始化完成')

    while rclpy.ok():
        for point in patrol.get_target_points():
            x, y, yaw = point[0], point[1], point[2]
            # 导航到目标点
            target_pose = patrol.get_pose_by_xyyaw(x, y, yaw)#合成位姿

            patrol.speach_text(text=f'准备前往目标点{x},{y}')
            patrol.nav_to_pose(target_pose)#导航到目标点
            patrol.speach_text(text=f"已到达目标点{x},{y},准备记录图像")
            patrol.record_image()#记录图像函数
            patrol.speach_text(text=f"图像记录完成")
    rclpy.shutdown()#不再需要的时候清理

if __name__ == '__main__':
    main()